/* ROBOT DEMO */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <strings.h>
#include <stdlib.h>
#include <pthread.h>
#include <semaphore.h>

#define BAUDRATE B57600  /* the baud rate */
#define MODEMDEVICE "/dev/ttyS0" /* port for serial port */
//#define MODEMDEVICE "/dev/rfcomm1" /* port for bluetooth connection */
#define _POSIX_SOURCE 1 /* POSIX compliant source */
void* movement(void* param);
void* serial(void* param);
int fd;
pthread_mutex_t key_mutex,sensor_mutex;
char new_move;
char check_sensor;
char start_buf[2],key_buf2[5];
unsigned char sens_buf[2];
char bumper;
int main()
{
	pthread_t tid_m,tid_s;
	pthread_attr_t attr_m,attr_s;
	pthread_attr_init(&attr_m);
	pthread_attr_init(&attr_s);
	pthread_mutex_init(&key_mutex,NULL);
	pthread_mutex_init(&sensor_mutex,NULL);
	printf("ROBOT DEMO\n");

//	int c, res;
//	char buf[6], buf2[6]; /* buffers used for read and write to irobot */
//	char key;

	struct termios oldtio,newtio;

//	printf("About to open modem device\n");
	/* Configure Serial port */
	/* Open modem device for reading and writing */
	fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY | O_NDELAY); 

//	printf("opened modem device\n");
	if (fd <0) {
		printf("Error Connecting Device‏‏\n");
		perror(MODEMDEVICE); 
		exit(-1); 
	}
	tcgetattr(fd,&oldtio); /* save current serial port settings */
	bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */
	
	newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
	newtio.c_iflag = IGNPAR | ICRNL;
	newtio.c_oflag = 0;
	newtio.c_lflag = ICANON;
	
	/* initialize all control characters */
	newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
	newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
	newtio.c_cc[VERASE]   = 0;     /* del */
	newtio.c_cc[VKILL]    = 0;     /* @ */
	newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */
	newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
	newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
	newtio.c_cc[VSWTC]    = 0;     /* '\0' */
	newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
	newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
	newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
	newtio.c_cc[VEOL]     = 0;     /* '\0' */
	newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
	newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
	newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
	newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
	newtio.c_cc[VEOL2]    = 0;     /* '\0' */

	/* now clean the modem line and activate the settings for the port */
	tcflush(fd, TCIFLUSH);
	tcsetattr(fd,TCSANOW,&newtio);
	pthread_mutex_lock(&sensor_mutex);
	start_buf[0]=128;
	start_buf[1]=132;
	write(fd,start_buf,2);
	pthread_create(&tid_m,&attr_m,movement,NULL);
	pthread_create(&tid_s,&attr_s,serial,NULL);

	sens_buf[0]=142;
	sens_buf[1]=9;
	while(1){
		sleep(1);
		check_sensor=1;
		printf("about to lock\n");
		pthread_mutex_lock(&sensor_mutex);
		printf("Left bumper = %d\n",bumper & 2);
		printf("Right bumper = %d\n",bumper & 1);
		check_sensor=0;
	}

	pthread_join(tid_m,NULL);
	pthread_join(tid_s,NULL);

	printf("Thread joined");

	}
void* movement(void* param){ 	
//	printf("About to send start sequence\n");
	/* initial start sequence*/
//	int c, res;
//	char buf[6], buf2[6]; /* buffers used for read and write to irobot */
	char key;

//	key_buf[0] = 128;
//	key_buf[1] = 131;
//	write(fd, buf, 2);

	printf("Press 'f' to go forward, 'b' to go backward‏‏, or 's' to stop\n");
	while (1) {
		key = getc(stdin); //gets character from stdin
		if (key == 'f') {
			printf("%c was keyed\n",key);
			//write bytes into global buffer that will tell robot to move
			key_buf2[0] = 137;
			key_buf2[1] = 0;
			key_buf2[2] = 100;
			key_buf2[3] = 128;
			key_buf2[4] = 0;
		}
		else if (key == 'b') {
			printf("%c was keyed\n",key);
			key_buf2[0] = 137;
			key_buf2[1] = 255;
			key_buf2[2] = 156;
			key_buf2[3] = 128;
			key_buf2[4] = 0;
		}
		else if (key == 's') {
			printf("%c was keyed\n",key);
			key_buf2[0] = 137;
			key_buf2[1] = 0;
			key_buf2[2] = 0;
			key_buf2[3] = 0;
			key_buf2[4] = 0;
		}
		else if (key == 'r') {
			printf("%c was keyed\n",key);
			key_buf2[0] = 137;
			key_buf2[1] = 0;
			key_buf2[2] = 100;
			key_buf2[3] = 255;
			key_buf2[4] = 255;
		}
		else if (key == 'l') {
			printf("%c was keyed\n",key);
			key_buf2[0] = 137;
			key_buf2[1] = 0;
			key_buf2[2] = 100;
			key_buf2[3] = 0;
			key_buf2[4] = 1;
		}
		else if (key == 'c') exit(0);//terminates the program
		else continue;   //prevents double-printing the 
				//"Sending Message" due to the CR
		new_move = 1;
		pthread_mutex_lock(&key_mutex);
//		printf("Sending Message to Robot...\n");
//		write(fd, buf, 2);//sequence packets for movement
//		write(fd, buf2, 5);
	}
	pthread_exit(0);
}
void* serial(void* param){
	while(1){
//		write(fd,start_buf,2);
		if(new_move){
			printf("Sending Message to Robot...\n");
//			write(fd,key_buf,2);
			write(fd,key_buf2,5);
			new_move = 0;
			pthread_mutex_unlock(&key_mutex);
		}
		if(check_sensor){
			printf("Asking Robot for sensors...\n");
			printf("Sens_buf has %u and %u\n",sens_buf[0],
				sens_buf[1]);
//			sleep(2);
			int write_val = write(fd,sens_buf,2);
			printf("just wrote, with return value %d\n",write_val);
			usleep(1000);
			int read_val = read(fd,&bumper,1);
			printf("read happened, returned %d\n",read_val);
			pthread_mutex_unlock(&sensor_mutex);
			printf("mutex unlocked\n");
		}
	}
}








